﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using SmartMathLibrary.LinearEquationSolvers.Preconditioners;

namespace SmartMathLibrary.LinearEquationSolvers
{
    /// <summary>
    /// This class provides the solving of a linear system of equation by using the BiConjugateStabilizedGradient 
    /// method. The algorithm, which will be used partly based on the IML++ library.
    /// </summary>
    [Serializable]
    public class BiConjugateStabilizedGradientSystemSolver : AbstractNonStationaryIterativeMethod
    {
        /// <summary>
        /// Initializes a new instance of the <see cref="BiConjugateStabilizedGradientSystemSolver"/> class.
        /// </summary>
        /// <param name="coefficientMatrix">The coefficient matrix of the equation system to solve.</param>
        /// <param name="resultVector">The result vector of the equation system to solve.</param>
        /// <param name="preconditioner">The preconditioner which should be used for the iterative method.</param>
        public BiConjugateStabilizedGradientSystemSolver(Matrix coefficientMatrix, GeneralVector resultVector,
                                                         IPreconditioner preconditioner)
            : base(coefficientMatrix, resultVector, preconditioner)
        {
        }

        /// <summary>
        /// Initializes a new instance of the <see cref="BiConjugateStabilizedGradientSystemSolver"/> class.
        /// </summary>
        /// <param name="equationSystem">The equation system to solve.</param>
        /// <param name="preconditioner">The preconditioner which should be used for the iterative method.</param>
        public BiConjugateStabilizedGradientSystemSolver(LinearEquationSystem equationSystem,
                                                         IPreconditioner preconditioner)
            : base(equationSystem, preconditioner)
        {
        }

        /// <summary>
        /// Solves the linear system of equation.
        /// </summary>
        /// <param name="startValues">The start values for x.</param>
        /// <param name="iterations">The maximum numbers of iterations to solve the system of equation.</param>
        /// <param name="precision">The precision of the result.</param>
        /// <returns>
        /// The vector which solves the linear system of equation Ax = b.
        /// </returns>
        public override GeneralVector SolveLinearSystem(GeneralVector startValues, int iterations, double precision)
        {
            //The algorithm partly based on the IML++ library of the National Institute of Standards and Technology.
            double residual;
            double rho_2 = 0;
            double alpha = 0;
            double omega = 0;
            double lenghtB = this.ResultVector.Length;
            GeneralVector s;
            GeneralVector p = new GeneralVector(startValues.Count);
            GeneralVector v = new GeneralVector(startValues.Count);
            GeneralVector r = this.ResultVector - this.CoefficientMatrix * startValues;
            GeneralVector rtilde = r;
            Matrix M = this.Preconditioner.ComputePreconditionMatrix();

            if (lenghtB == 0.0)
            {
                lenghtB = 1;
            }

            if ((residual = r.Length / lenghtB) <= precision)
            {
                this.relativeError = residual;
                this.neededIterations = 0;
                this.precisionError = false;

                return startValues;
            }

            for (int i = 0; i < iterations; i++)
            {
                double rho_1 = GeneralVector.ComputeDotProduct(rtilde, r);

                if (rho_1 == 0)
                {
                    this.relativeError = r.Length / lenghtB;
                    this.neededIterations = 0;
                    this.precisionError = false;

                    return startValues;
                }

                if (i == 0)
                {
                    p = r;
                }
                else
                {
                    double beta = (rho_1 / rho_2) * (alpha / omega);

                    p = r + beta * (p - omega * v);
                }

                GeneralVector phat = M * p;

                v = this.CoefficientMatrix * phat;
                alpha = rho_1 / GeneralVector.ComputeDotProduct(rtilde, v);
                s = r - alpha * v;

                if ((residual = s.Length / lenghtB) < precision)
                {
                    startValues += alpha * phat;

                    this.relativeError = residual;
                    this.neededIterations = i + 1;
                    this.precisionError = false;

                    return startValues;
                }

                GeneralVector shat = M * s;
                GeneralVector t = this.CoefficientMatrix * shat;

                omega = GeneralVector.ComputeDotProduct(t, s) / GeneralVector.ComputeDotProduct(t, t);
                startValues += alpha * phat + omega * shat;
                r = s - omega * t;
                rho_2 = rho_1;

                if ((residual = r.Length / lenghtB) < precision)
                {
                    this.relativeError = residual;
                    this.neededIterations = i + 1;
                    this.precisionError = false;

                    return startValues;
                }

                if (omega == 0)
                {
                    this.relativeError = r.Length / lenghtB;
                    this.neededIterations = i + 1;
                    this.precisionError = false;

                    return startValues;
                }
            }

            this.relativeError = residual;
            this.neededIterations = iterations;
            this.precisionError = true;

            return startValues;
        }
    }
}